#include <stdio.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include <common/buffer.h>
#include <common/arraysize.h>
#include <common/navmath.h>
#include <common/Thread.h>
#include <serialport/protocol.h>
#include <serialport/serialport.h>
#include "simlink.h"

const int SimLink::bufsize_ = 256;

const uint8_t SimLink::addr_ = 0xF0;
const uint8_t SimLink::id_ = 0x00;


SimLink::SimLink(SerialPort *port)
	: port_(port)
	, buf_(new uint8_t[bufsize_])
	, fail_cnt_(0)
{
	assert(buf_ != NULL);
}


SimLink::~SimLink() {
	delete[] buf_;
}


void SimLink::run() {
	Period period(20);	// thread period is 20 ms
	
	while (!isExitFlag()) {
		update();
		period.wait();
	}
}


void SimLink::update() {
	Protocol proto(port_, buf_, bufsize_);

	int size = request(proto.getDataPointer(), proto.getMaxDataSize());

	if (proto.request(addr_, id_, size)) {
		response(proto.getDataPointer(), size);
		fail_cnt_ = 0;
		Thread::sleep(1);
	} else {
		fail();
		Thread::sleep(10);	// wait for the rest bytes and drop
	}

	port_->clean();
}


int SimLink::request(uint8_t *data, int datasize) {
	actuator_controls_.update();

	Buffer buf(data, datasize);

	buf.putFloat32(actuator_controls_->control[0]);	// ailerons
	buf.putFloat32(actuator_controls_->control[1]);	// elevator
	buf.putFloat32(actuator_controls_->control[2]);	// rudder
	buf.putFloat32(actuator_controls_->control[3]);	// throttle

	int flaps = (int)actuator_controls_->control[4] + 1;	// -1.0f..+1.0f -> 0, 1, 2
	const float flaps_pos[] = {0.0f, 0.349f, 1.0f};
	buf.putFloat32(flaps_pos[flaps]);

	buf.putFloat32(actuator_controls_->control[5]);	// brakes
	buf.putFloat32(actuator_controls_->control[6]);	// parachute

	buf.putUint8(0/*static_cast<int8_t>(p().payload.gymbal.azimuthCtrl * 127.0f)*/);
	buf.putUint8(0/*static_cast<int8_t>(p().payload.gymbal.elevationCtrl * 127.0f)*/);
	buf.putUint8(0/*static_cast<int8_t>(p().payload.gymbal.fovCtrl * 127.0f)*/);

	return buf.getIndexForPut();
}


void SimLink::response(uint8_t *data, int datasize) {
	Buffer buf(data, datasize);
	sim_sensors_->roll = buf.getFloat32();
	sim_sensors_->pitch = buf.getFloat32();
	sim_sensors_->heading = buf.getFloat32();
	sim_sensors_->gyro[0] = buf.getFloat32();
	sim_sensors_->gyro[1] = buf.getFloat32();
	sim_sensors_->gyro[2] = buf.getFloat32();
	sim_sensors_->accel[0] = buf.getFloat32();
	sim_sensors_->accel[2] = -buf.getFloat32();
	sim_sensors_->accel[1] = buf.getFloat32();

	sim_sensors_->airspeed = buf.getFloat32();
	sim_sensors_->baroalt = buf.getFloat32();
	sim_sensors_->vspeed = buf.getFloat32();

	sim_sensors_->directAlt = buf.getFloat32();

	sim_sensors_->latitude = buf.getFloat64();
	sim_sensors_->longitude = buf.getFloat64();

	sim_sensors_->altitude = buf.getFloat32();
	sim_sensors_->groundSpeed = buf.getFloat32();
	sim_sensors_->course = buf.getFloat32();

	/*year*/ buf.getUint16();
	/*month*/ buf.getUint8();
	/*day*/ buf.getUint8();
	/*hour*/ buf.getUint8();
	/*minutes*/ buf.getUint8();
	/*seconds*/ buf.getUint8();

	sim_sensors_->rpm = buf.getUint16();
	sim_sensors_->temp1 = static_cast<int16_t>(buf.getUint16());
	sim_sensors_->temp2 = sim_sensors_->temp1;
	sim_sensors_->fuel_level = buf.getUint8() * 0.01f;

	sim_sensors_->wow = buf.getUint8() != 0;

	if (sim_sensors_->failures.getWow())
		sim_sensors_->wow = true;

	/*p().angles.aoa =*/ buf.getFloat32();
	/*p().angles.slip =*/ buf.getFloat32();

	/*p().ois.azimuth =*/ buf.getUint16() * 0.01f;
	/*p().ois.elevation =*/ buf.getUint16() * 0.01f;
	/*p().ois.zoom =*/ buf.getUint16() * 0.01f;

	sim_sensors_->failures.set(buf.getUint16());

	sim_sensors_->staticPressure = buf.getFloat64();
	sim_sensors_->diffPressure = buf.getFloat64();
	sim_sensors_->airTemp = buf.getFloat32();

	sim_sensors_->active = true;
	sim_sensors_.publish();
}


void SimLink::fail() {
	if (++fail_cnt_ >= 10) {
		fail_cnt_ = 10;
		sim_sensors_->active = false;
		sim_sensors_.publish();
	}
}

